package pl.edu.agh.ros.node;

import org.ros.internal.node.service.ServiceResponseBuilder;
import org.ros.node.DefaultNodeFactory;
import org.ros.node.Node;
import org.ros.node.NodeConfiguration;
import org.ros.node.NodeMain;
import org.ros.node.service.ServiceServer;
import org.ros.service.test_ros.AddTwoInts;

import com.google.common.base.Preconditions;

public class RosServiceServer implements NodeMain {

	private static final String SERVICE_NAME = "/test_service"; //$NON-NLS-1$
	private static final String SERVICE_TYPE = "test_ros/AddTwoInts"; //$NON-NLS-1$
	private Node node;

	@Override
	public void main(NodeConfiguration configuration) {
		Preconditions.checkState(node == null);
		Preconditions.checkNotNull(configuration);
		try {
			System.out.println("Starting a Testing Service Server Node........"); //$NON-NLS-1$
			node = new DefaultNodeFactory().newNode("test_service_server", configuration); //$NON-NLS-1$

			ServiceServer<AddTwoInts.Request, AddTwoInts.Response> server = node.newServiceServer(SERVICE_NAME,
					SERVICE_TYPE, new ServiceResponseBuilder<AddTwoInts.Request, AddTwoInts.Response>() {
						@Override
						public AddTwoInts.Response build(AddTwoInts.Request request) {
							AddTwoInts.Response response = new AddTwoInts.Response();
							if (request.b % 2 == 1) {
								try {
									Thread.sleep(10000);
								} catch (InterruptedException e) {
									// TODO Auto-generated catch block
									e.printStackTrace();
								}
							}
							response.sum = request.a + request.b;
							return response;
						}
					});
			server.awaitRegistration();
		} catch (Exception e) {
			if (node != null) {
				node.getLog().fatal(e);
			} else {
				e.printStackTrace();
			}
		}
	}

	@Override
	public void shutdown() {
		node.shutdown();
		node = null;
	}

}
